#!/usr/bin/env python3
import cv2
import threading
import numpy as np
# img_cube1 = None
# img_cube2 = None
# img_cube3 = None
# img_cube4 = None
# img_cube5 = None

img_flag1 = 0
img_flag2 = 0
img_flag3 = 0
img_flag4 = 0
img_flag5 = 0
soving_flag = 0
sovingdown_flag = 0
cubestate = ""
def cube_recognize():
    global img_cube1,img_cube2,img_cube3,img_cube4,img_cube5,cubestate
    img0 = img_cube1
    img1 = img_cube2
    img2 = img_cube3
    img3 = img_cube4
    img4 = img_cube5
    sta_cube = []

    block_l1 = (309, 221)
    block_l2 = (246, 219)
    block_l3 = (180, 219)
    block_l4 = (314, 258)
    block_l5 = (243, 259)
    block_l6 = (173, 256)
    block_l7 = (322, 306)
    block_l8 = (242, 306)
    block_l9 = (166, 304)

    block_r1 = (322, 364)
    block_r2 = (242, 363)
    block_r3 = (163, 361)
    block_r4 = (313, 409)
    block_r5 = (245, 411)
    block_r6 = (172, 411)
    block_r7 = (310, 447)
    block_r8 = (244, 448)
    block_r9 = (181, 448)

    sta_cube.append(img4[block_l1])        
    sta_cube.append(img4[block_l2])
    sta_cube.append(img4[block_l3])
    sta_cube.append(img4[block_l4])
    sta_cube.append(img4[block_l5])
    sta_cube.append(img4[block_l6])
    sta_cube.append(img4[block_l7])
    sta_cube.append(img4[block_l8])
    sta_cube.append(img4[block_l9])

    sta_cube.append(img1[block_r9])        
    sta_cube.append(img1[block_r8])
    sta_cube.append(img1[block_r7])
    sta_cube.append(img1[block_r6])
    sta_cube.append(img1[block_r5])
    sta_cube.append(img1[block_r4])
    sta_cube.append(img1[block_r3])
    sta_cube.append(img1[block_r2])
    sta_cube.append(img1[block_r1])

    sta_cube.append(img4[block_r1])        
    sta_cube.append(img4[block_r2])
    sta_cube.append(img4[block_r3])
    sta_cube.append(img4[block_r4])
    sta_cube.append(img4[block_r5])
    sta_cube.append(img4[block_r6])
    sta_cube.append(img4[block_r7])
    sta_cube.append(img4[block_r8])
    sta_cube.append(img4[block_r9])

    sta_cube.append(img3[block_l9])        
    sta_cube.append(img3[block_l8])
    sta_cube.append(img3[block_l7])
    sta_cube.append(img3[block_l6])
    sta_cube.append(img3[block_l5])
    sta_cube.append(img3[block_l4])
    sta_cube.append(img3[block_l3])
    sta_cube.append(img3[block_l2])
    sta_cube.append(img3[block_l1])

    sta_cube.append(img0[block_r9])        
    sta_cube.append(img0[block_r8])
    sta_cube.append(img0[block_r7])
    sta_cube.append(img0[block_r6])
    sta_cube.append(img0[block_r5])
    sta_cube.append(img0[block_r4])
    sta_cube.append(img0[block_r3])
    sta_cube.append(img0[block_r2])
    sta_cube.append(img0[block_r1])

    sta_cube.append(img2[block_r9])        
    sta_cube.append(img2[block_r8])
    sta_cube.append(img2[block_r7])
    sta_cube.append(img2[block_r6])
    sta_cube.append(img2[block_r5])
    sta_cube.append(img2[block_r4])
    sta_cube.append(img2[block_r3])
    sta_cube.append(img2[block_r2])
    sta_cube.append(img2[block_r1])
    print(sta_cube)
    # piex1 = img[100,100]
    # piex2 = img[120,120]
    # piex3 = img[150,150]
    # piex4 = img[48,29]
    # piex5 = img[89,102]
    # piex6 = img[89,300]
    # piex10 = [piex1, piex2, piex3, piex4, piex5, piex6]
    # # print(piex1)
    # # print(piex2)
    # # print(piex3)
    # # print(piex4)
    sta_cube = np.float32(sta_cube)

    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10, 1.0)
    flags = cv2.KMEANS_RANDOM_CENTERS
    ret, best_labels, centers = cv2.kmeans(sta_cube, 6, None, criteria, 10, flags)
    # print(best_labels)
    # print(len(best_labels))

    state = ""

    img4 = cv2.circle(img4, exchange(block_l1), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l2), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l3), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l4), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l5), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l6), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l7), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l8), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_l9), 3, (255, 0, 0), 2)
    cv2.imshow('img4', img4)

    img1 = cv2.circle(img1, exchange(block_r9), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r8), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r7), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r6), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r5), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r4), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r3), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r2), 3, (255, 0, 0), 2)
    img1 = cv2.circle(img1, exchange(block_r1), 3, (255, 0, 0), 2)
    cv2.imshow('img1', img1)

    img4 = cv2.circle(img4, exchange(block_r1), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r2), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r3), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r4), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r5), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r6), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r7), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r8), 3, (255, 0, 0), 2)
    img4 = cv2.circle(img4, exchange(block_r9), 3, (255, 0, 0), 2)
    cv2.imshow('img4', img4)

    img3 = cv2.circle(img3, exchange(block_l9), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l8), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l7), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l6), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l5), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l4), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l3), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l2), 3, (255, 0, 0), 2)
    img3 = cv2.circle(img3, exchange(block_l1), 3, (255, 0, 0), 2)
    cv2.imshow('img3', img3)


    img0 = cv2.circle(img0, exchange(block_r9), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r8), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r7), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r6), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r5), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r4), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r3), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r2), 3, (255, 0, 0), 2)
    img0 = cv2.circle(img0, exchange(block_r1), 3, (255, 0, 0), 2)
    cv2.imshow('img0', img0)



    img2 = cv2.circle(img2, exchange(block_r9), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r8), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r7), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r6), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r5), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r4), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r3), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r2), 3, (255, 0, 0), 2)
    img2 = cv2.circle(img2, exchange(block_r1), 3, (255, 0, 0), 2)
    cv2.imshow('img2', img2)
    # cv2.waitKey(2)

    ff1 = best_labels[4]
    ff2 = best_labels[13]
    ff3 = best_labels[22]
    ff4 = best_labels[31]
    ff5 = best_labels[40]
    ff6 = best_labels[49]


    for i in best_labels:
        if(i == ff1):
            state=state+'U'
        elif(i == ff2):
            state=state+'R'
        elif(i == ff3):
            state=state+'F'
        elif(i == ff4):
            state=state+'D'
        elif(i == ff5):
            state=state+'L'
        elif(i == ff6):
            state=state+'B'
    # print(state)
    # return state
    cubestate = state


def update_img_flag(ImgNum):
    global img_flag1,img_flag2,img_flag3,img_flag4,img_flag5,soving_flag
    if (ImgNum == 1):
        img_flag1 = 1
    elif (ImgNum == 2):
        img_flag2 = 1
    elif (ImgNum == 3):
        img_flag3 = 1
    elif (ImgNum == 4):
        img_flag4 = 1
    elif (ImgNum == 5):
        img_flag5 = 1
    elif (ImgNum == 0):
        soving_flag = 1
def Thread_UpdateImg():
    global img_flag1,img_flag2,img_flag3,img_flag4,img_flag5,soving_flag,sovingdown_flag
    global img_cube1,img_cube2,img_cube3,img_cube4,img_cube5
    cap = cv2.VideoCapture(1)
    while (True):
        ret,img_cube,  = cap.read()
        if (img_flag1 == 1):
            img_cube1  = img_cube
            # cv2.imshow("img1",img_cube1)
            img_flag1 = 0
        if (img_flag2 == 1):
            img_cube2  = img_cube
            # cv2.imshow("img2",img_cube2)
            img_flag2 = 0
        if (img_flag3 == 1):
            img_cube3  = img_cube
            # cv2.imshow("img3",img_cube3)
            img_flag3 = 0
        if (img_flag4 == 1):
            img_cube4  = img_cube
            # cv2.imshow("img4",img_cube4)
            img_flag4 = 0
        if (img_flag5 == 1):
            img_cube5  = img_cube
            # cv2.imshow("img5",img_cube5)
            img_flag5 = 0
        if (img_flag5 == 1):
            img_cube5  = img_cube
            # cv2.imshow("img5",img_cube5)
            img_flag5 = 0
        if (soving_flag == 1):
            # img_cube5  = img_cube
            cube_recognize()
            # cv2.imshow("img5",img_cube5)
            sovingdown_flag  =1
            soving_flag = 0
        cv2.imshow("cube",img_cube)
        cv2.waitKey(20)
def exchange(val):
    return (val[1], val[0])

queue_t = threading.Thread(target=Thread_UpdateImg)
queue_t.daemon = True
queue_t.start()
print("vision module start working!")
